/*
  CAN总线上的讄1�7
  将所有CAN总线上挂载的设抽象成丢�设进行配和控刄1�7
*/


/* Includes ----------------------------------------------------------------- */
#include "can_use.h"
#include "user_math.h"
#include "device.h"
#define CAN_MOTOR_ENC_RES (8191)  /* 电机编码器分辨率 */
#define CAN_MOTOR_CUR_RES (16384) /* 电机转矩电流分辨玄1�7 */


#define CAN_GM6020_CTRL_ID_BASE (0x1ff)
#define CAN_VESC_CTRL_ID_BASE (0x300)


#define CAN_MOTOR_TX_BUF_SIZE (8)
#define CAN_MOTOR_RX_BUF_SIZE (8)

/* 电机朢�大电流绝对��1�7 */
#define CAN_GM6020_MAX_ABS_CUR (1)
#define CAN_M3508_MAX_ABS_CUR (20)
#define CAN_M2006_MAX_ABS_CUR (10)

static CAN_RawRx_t raw_rx1;
static CAN_RawRx_t raw_rx2;
 CAN_RawTx_t raw_tx;

static CAN_t *gcan;//有什么用＄1�7


/* Private function  -------------------------------------------------------- */

static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
                             const uint8_t *raw) {
  uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
  int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);

  feedback->rotor_angle = raw_angle / (float)CAN_MOTOR_ENC_RES * 360.0f;
  feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
  feedback->torque_current =
      raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
  feedback->temp = raw[6];
}

//Vesc反馈函数待修改
static void CAN_VescMotor_Decode(CAN_MotorFeedback_t *feedback,
                             const uint8_t *raw) {
  uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
  int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);

  feedback->rotor_angle = raw_angle / (float)CAN_MOTOR_ENC_RES * M_2PI;
  feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
  feedback->torque_current =
      raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
  feedback->temp = raw[6];
}


static void CAN_CAN1RxFifoMsgPendingCallback(void) {
  HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_1), CAN_RX_FIFO0,
                       &raw_rx1.rx_header, raw_rx1.rx_data);
  osMessageQueuePut(gcan->msgq_raw, &raw_rx1, 0, 0);
}

static void CAN_CAN2RxFifoMsgPendingCallback(void) {
  HAL_CAN_GetRxMessage(BSP_CAN_GetHandle(BSP_CAN_2), CAN_RX_FIFO1,
                       &raw_rx2.rx_header, raw_rx2.rx_data);
  osMessageQueuePut(gcan->msgq_raw, &raw_rx2, 0, 0);
}


/* Exported functions ------------------------------------------------------- */
int8_t CAN_Init(CAN_t *can, const CAN_Params_t *param) {
  if (can == NULL) return DEVICE_ERR_NULL;

  can->msgq_raw = osMessageQueueNew(32, sizeof(CAN_RawRx_t), NULL);

  can->param = param;

  CAN_FilterTypeDef can_filter = {0};

  can_filter.FilterBank = 0;
  can_filter.FilterIdHigh = 0;
  can_filter.FilterIdLow = 0;
  can_filter.FilterMode = CAN_FILTERMODE_IDMASK;
  can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter.FilterMaskIdHigh = 0;
  can_filter.FilterMaskIdLow = 0;
  can_filter.FilterActivation = ENABLE;
  can_filter.SlaveStartFilterBank = 14;
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO0;

  HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_1), &can_filter);
  HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_1));
  BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
                           CAN_CAN1RxFifoMsgPendingCallback);
  HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_1),
                               CAN_IT_RX_FIFO0_MSG_PENDING);

  can_filter.FilterBank = 14;
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO1;

  HAL_CAN_ConfigFilter(BSP_CAN_GetHandle(BSP_CAN_2), &can_filter);
  HAL_CAN_Start(BSP_CAN_GetHandle(BSP_CAN_2));
  BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
                           CAN_CAN2RxFifoMsgPendingCallback);
  HAL_CAN_ActivateNotification(BSP_CAN_GetHandle(BSP_CAN_2),
                               CAN_IT_RX_FIFO1_MSG_PENDING);

  gcan = can;
  return DEVICE_OK;
}



int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
                         CAN_t *can) {
  if (output == NULL) return DEVICE_ERR_NULL;

  int16_t motor1, motor2, motor3, motor4;
  switch (group) {
    case CAN_MOTOR_CHASSIS6020:
      motor1 =
          (int16_t)(output->chassis6020.named.m1);
      motor2 =
          (int16_t)(output->chassis6020.named.m2);
      motor3 =
          (int16_t)(output->chassis6020.named.m3);
      motor4 =
          (int16_t)(output->chassis6020.named.m4);

      raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
      raw_tx.tx_header.IDE = CAN_ID_STD;
      raw_tx.tx_header.RTR = CAN_RTR_DATA;
      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;

      raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
      raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
      raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
      raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
      raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
      raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
      raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
      raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);

      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
                           &raw_tx.tx_header, raw_tx.tx_data,
                           &(can->mailbox.chassis));
      break;
    default:
      break;
  }
  return DEVICE_OK;
}
												 

int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){
    if (output == NULL) return DEVICE_ERR_NULL;
    int Byte[4];
   Vesc_ByteGet raw[4];
    switch (group) {
     case CAN_MOTOR_CHASSIS5065:
			 //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
			raw[0].as_array = output->chassis5065.named.m1;
		  raw[1].as_array = output->chassis5065.named.m2;
		  raw[2].as_array = output->chassis5065.named.m3;
		  raw[3].as_array = output->chassis5065.named.m4;
		 
		 
     for(int i=0 ; i < 4 ; i ++)
     {
			 
			 if(i==2)  //此处可能同时发送四个vesc导致只有三个轮子接收到数据 故加上delay
			 {
				osDelay(1);
			 }
			 //将单个电机的期望输出值通过联合体拆分
      Byte[0] = raw[i].byte.byte1;
      Byte[1] = raw[i].byte.byte2;
			Byte[2] = raw[i].byte.byte3;
			Byte[3] = raw[i].byte.byte4;
			 
      raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE;
      raw_tx.tx_header.IDE = CAN_ID_EXT;
      raw_tx.tx_header.RTR = CAN_RTR_DATA;
      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;

      raw_tx.tx_data[0] = Byte[3];
      raw_tx.tx_data[1] = Byte[2];
      raw_tx.tx_data[2] = Byte[1];
      raw_tx.tx_data[3] = Byte[0];
      raw_tx.tx_data[4] = 0;
      raw_tx.tx_data[5] = 0;
      raw_tx.tx_data[6] = 0;
      raw_tx.tx_data[7] = 0;

      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis5065),
                           &raw_tx.tx_header, raw_tx.tx_data,
                           &(can->mailbox.chassis));

     }

    }



  return DEVICE_OK;
}


int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
  if (can == NULL) return DEVICE_ERR_NULL;
  if (can_rx == NULL) return DEVICE_ERR_NULL;

  uint32_t index;
  switch (can_rx->rx_header.StdId) {
    case CAN_G6020_AgvM1:
    case CAN_G6020_AgvM2:
    case CAN_G6020_AgvM3:
    case CAN_G6020_AgvM4:
    index = can_rx->rx_header.StdId - CAN_G6020_AgvM1;
      can->recive_flag |= 1 << (index);
      CAN_DJIMotor_Decode(&(can->motor.chassis6020.as_array[index]), can_rx->rx_data);
      break;
    case CAN_VESC5065_M1:
    case CAN_VESC5065_M2:
    case CAN_VESC5065_M3:
    case CAN_VSEC5065_M4:



      break;

    default:
      break;
  }
  return DEVICE_OK;
}


bool_t CAN_CheckFlag(CAN_t *can, uint32_t flag) {
  if (can == NULL) return false;
  return (can->recive_flag & flag) == flag;
}

int8_t CAN_ClearFlag(CAN_t *can, uint32_t flag) {
  if (can == NULL) return DEVICE_ERR_NULL;
  can->recive_flag &= ~flag;
  return DEVICE_OK;
}
